Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add Vector Object server #4680

Open
wants to merge 22 commits into
base: main
Choose a base branch
from
Open

Add Vector Object server #4680

wants to merge 22 commits into from

Conversation

ajtudela
Copy link
Contributor

Basic Info

Info Please fill out this column
Ticket(s) this addresses #3017, #3930
Primary OS tested on Ubuntu 24.04
Robotic platform tested on
Does this PR contain AI generated software? No

Description of contribution in a few bullet points

Just the commits from #3930

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

  • Check that any new parameters added are updated in docs.nav2.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

AlexeyMerzlyakov and others added 20 commits September 17, 2024 14:27
@AlexeyMerzlyakov @ajtudela
Add Vector Object server
4b8c7eb 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Meet review comments
f02a925 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
@AlexeyMerzlyakov @ajtudela
Simplify shapes param configuring
8a506bb 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Rename getROSParameter() to getParameter()
64c63ce 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Return back getMaskData() to nav2_costmap_2d
75ccc4c 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Add composition node support
4467b38 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
@AlexeyMerzlyakov @ajtudela
Remove redundant methods
718a5b3 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @SteveMacenski @ajtudela
Update nav2_map_server/src/vo_server/vector_object_server.cpp
4465258 
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Avoid shapes clearing
79bae1c 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Optimize switchMapUpdate() method
24a87c9 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Switch to vector of shapes
b4b303f 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Minor fixes
724d848 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Meet review comments
3a02878 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Move isPointInside algorithm to nav2_util
1067a56 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Testcases covering new functionality
ebfa990 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Fix linting issues
b33d88a 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Adjust for Vector Objects demonstration
c0025ec 
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Code clean-up
d579516 
* Corrected headers
* Functions ordering
* Comment fixes

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@AlexeyMerzlyakov @ajtudela
Additional code facelift
9a05aad 
* Correct licensing years
* Fix Vector Object server dependencies
* Funcion rename for better readability
* Improve/fix comments

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
@ajtudela
Minor fixing after rebase
323410a 
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@codecov Codecov
Copy link

codecov bot commented Sep 17, 2024 •
edited
Loading

Codecov Report

Attention: Patch coverage is 96.11111% with 21 lines in your changes missing coverage. Please review.

Files with missing lines Patch % Lines
..._map_server/src/vo_server/vector_object_shapes.cpp 95.47% 10 Missing ⚠️
..._map_server/src/vo_server/vector_object_server.cpp 96.77% 7 Missing ⚠️
nav2_map_server/src/vo_server/main.cpp 0.00% 4 Missing ⚠️
Files with missing lines Coverage Δ
nav2_collision_monitor/src/polygon.cpp 97.27% <100.00%> (-0.12%) ⬇️
..._costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp 100.00% <ø> (ø)
...nav2_costmap_2d/costmap_filters/costmap_filter.hpp 66.66% <ø> (ø)
...stmap_2d/plugins/costmap_filters/binary_filter.cpp 94.73% <100.00%> (ø)
...tmap_2d/plugins/costmap_filters/costmap_filter.cpp 87.69% <ø> (-1.65%) ⬇️
...tmap_2d/plugins/costmap_filters/keepout_filter.cpp 94.18% <100.00%> (ø)
...ostmap_2d/plugins/costmap_filters/speed_filter.cpp 95.12% <100.00%> (ø)
nav2_costmap_2d/plugins/obstacle_layer.cpp 76.97% <100.00%> (+0.07%) ⬆️
nav2_costmap_2d/src/costmap_2d.cpp 87.55% <100.00%> (+0.05%) ⬆️
...er/include/nav2_map_server/vector_object_utils.hpp 100.00% <100.00%> (ø)
... and 6 more

... and 6 files with indirect coverage changes

🚨 Try these New Features:

@SteveMacenski SteveMacenski mentioned this pull request Sep 17, 2024
Closed
7 tasks
@ajtudela
Copy link
Contributor Author

ajtudela commented Nov 7, 2024

Hi @SteveMacenski, if you have a moment, could you look at this and let me know what changes are needed.

Thanks very much!

@SteveMacenski
Copy link
Member

SteveMacenski commented Nov 13, 2024 •
edited
Loading

I took a look at about ~2/3 of it this afternoon. I didn't quite get to all of it, so I'll finish my review next time I get a chance :-)

In glancing through the old PR, I came up on this comment #3930 (comment) -- what do you think?

SteveMacenski
SteveMacenski requested changes Nov 14, 2024
View reviewed changes
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done! Largely nitpicks :-) Can you go through the annotations for test coverage where missing and fill in a few of the gaps that are relatively easy to do?

nav2_map_server/launch/vector_object_server.launch.py
'container_name', default_value='nav2_container',
description='the name of conatiner that nodes will load in if use composition')

configured_params = ParameterFile(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Check this against main: is this still how we handle this? I think this may have changed

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I checked and it's the same in other launch files in main.

nav2_map_server/launch/vector_object_server.launch.py
PushROSNamespace(
condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')),
namespace=namespace),
Node(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Put the lifecycle manager last so it doesn't try to bring servers up that aren't yet initialized. Ditto in the composition version below

nav2_map_server/src/vo_server/vector_object_server.cpp

bool VectorObjectServer::obtainParams()
{
// Main ROS-parameters
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Usually we do a auto node = shared_from_this() so that the node is used in all the calls below (its more natural looking)

nav2_map_server/src/vo_server/vector_object_server.cpp Outdated
shared_from_this(), "shapes", std::vector<std::string>()).as_string_array();
for (std::string shape_name : shape_names) {
std::string shape_type;

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove line

nav2_map_server/src/vo_server/vector_object_server.cpp Outdated
}
shapes_.push_back(circle);
} else {
RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In the log, give the options so they can fix it fast

nav2_map_server/src/vo_server/vector_object_server.cpp Outdated

for (auto shape : shapes_) {
shape->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y);
if (min_p_x < min_x) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std::min and std::max would be more concise

nav2_map_server/src/vo_server/vector_object_server.cpp
if (!map_) {
map_ = std::make_shared<nav_msgs::msg::OccupancyGrid>();
}
if (
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add space between if statements

nav2_map_server/src/vo_server/vector_object_server.cpp Outdated

void VectorObjectServer::publishMap()
{
if (map_) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Check also if map_pub_'s number of subscribers is > 0 to bother with the operation at all

nav2_map_server/src/vo_server/vector_object_server.cpp
publishMap();
}

void VectorObjectServer::switchMapUpdate()
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am a little baffled on this method.

Do you have an idea why we create a map_timer_ for each object who's frame ID doesn't match the current frame ID? It seems to override each other. We then instantly cancel it if it exists too... and processMap

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not exactly sure. I read this in the doc PR:

  • If at least one of the shape is set in different than map's frame, dynamic update model to be enabled: this shape can move over the time, output map will be published dynamically with a given rate.
  • If all shapes are set in the same as map frame, map is being published/updated once: during Vector Object server startup and per each shape changing call (AddShapes.srv or RemoveShapes.srv).

nav2_map_server/src/vo_server/vector_object_shapes.cpp Outdated
max_y = std::numeric_limits<double>::lowest();

for (auto point : polygon_->points) {
if (point.x < min_x) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std min/max

@ajtudela
Rename vector object server
1a8d07f 
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
@ajtudela
Minor changes
906ed7c 
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@SteveMacenski SteveMacenski SteveMacenski requested changes

Requested changes must be addressed to merge this pull request.

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
3 participants
@ajtudela @SteveMacenski @AlexeyMerzlyakov